Multivariate KF Examples
Example 1 – vehicle location estimation
We design a six-dimensional Kalman Filter without control input. That is, we estimate a vehicle’s location on the XY plane. The vehicle has an on-board location sensor that reports X and Y coordinates of the system. We assume constant acceleration dynamics.
Kalman Filter equations
There is no control input so: \(\textbf{u} = 0\)
The state extrapolation equation: \[\hat{\textbf{x}}_{n+1, n} = \textbf{F} \hat{\textbf{x}}_{n, n}\]
The system state \(x_{n}\) is defined by: \[ \textbf{x}_{n} = \begin{bmatrix} x_{n} \\ \dot{x}_{n} \\ \ddot{x}_{n} \\ y_{n} \\ \dot{y}_{n} \\ \ddot{y}_{n} \end{bmatrix} \]
So the extrapolated vehicle state for time n + 1 can be described by:
\(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \\ \hat{\ddot{x}}_{n+1,n} \\ \hat{y}_{n+1,n} \\ \hat{\dot{y}}_{n+1,n} \\ \hat{\ddot{y}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t & 0.5 \Delta t^2 & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5 \Delta t^2\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} \end{array}\)
The covariance extrapolation equation:
The estimate covariance matrix is, assuming that the estimation error in \(X\) and \(Y\) axes are not correlated: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} \]
The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \\ \end{bmatrix} \\ &= \sigma^2_{a} \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\ \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\ 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \\ \end{bmatrix} \end{align*}\]
The measurement equation:
The measurement provides us only \(X\) and \(Y\) coordinates of the vehicle: \[ z_{n} = \begin{bmatrix} x_{n, measured} \\ y_{n, measured} \end{bmatrix} \]
The dimension of \(z_{n}\) is \(2×1\) and the dimension of \(x_{n}\) is \(6×1\). Therefore the dimension of the observation matrix \(H\) shall be \(2 × 6\). \[ \textbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]
The measurement uncertainty, assuming that the \(x\) and \(y\) measurements are uncorrelated: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} & 0 \\ 0 & \sigma^2_{y_{m}} \end{bmatrix} \]
The subscript \(m\) is for measurement uncertainty, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]
So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.
Numerical example
Let us assume a vehicle moving in a straight line in the X direction with a constant velocity. After traveling 400 meters the vehicle turns right, with a turning radius of 300 meters. During the turning maneuver, the vehicle experiences acceleration due to the circular motion (angular acceleration).
# The measurements period (s)
deltaT = 1
# The random acceleration standard deviation (m/s^2)
sigma_a = 0.2
# The measurement error standard deviation (m)
sigma_xm = 3
sigma_ym = 3
# The state transition matrix F would be
vec1 <- c(1, 1, 0.5, 0, 0, 0)
vec2 <- c(0, 1, 1, 0, 0, 0)
vec3 <- c(0, 0, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1, 1, 0.5)
vec5 <- c(0, 0, 0, 0, 1, 1)
vec6 <- c(0, 0, 0, 0, 0, 1)
transMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6); transMat## [,1] [,2] [,3] [,4] [,5] [,6]
## vec1 1 1 0.5 0 0 0.0
## vec2 0 1 1.0 0 0 0.0
## vec3 0 0 1.0 0 0 0.0
## vec4 0 0 0.0 1 1 0.5
## vec5 0 0 0.0 0 1 1.0
## vec6 0 0 0.0 0 0 1.0
# The process noise matrix Q would be
vec1 <- c(1/4, 1/2, 1/2, 0, 0, 0)
vec2 <- c(1/2, 1, 1, 0, 0, 0)
vec3 <- c(1/2, 1, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1/4, 1/2, 1/2)
vec5 <- c(0, 0, 0, 1/2, 1, 1)
vec6 <- c(0, 0, 0, 1/2, 1, 1)
processNoiseMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6) * 0.2^2
processNoiseMat## [,1] [,2] [,3] [,4] [,5] [,6]
## vec1 0.01 0.02 0.02 0.00 0.00 0.00
## vec2 0.02 0.04 0.04 0.00 0.00 0.00
## vec3 0.02 0.04 0.04 0.00 0.00 0.00
## vec4 0.00 0.00 0.00 0.01 0.02 0.02
## vec5 0.00 0.00 0.00 0.02 0.04 0.04
## vec6 0.00 0.00 0.00 0.02 0.04 0.04
## [,1] [,2]
## [1,] 9 0
## [2,] 0 9
## [,1] [,2] [,3] [,4] [,5] [,6]
## [1,] 1 0 0 0 0 0
## [2,] 0 0 0 1 0 0
# The set of 35 noisy measurements
x <- c(301.5, 298.23, 297.83, 300.42, 301.94, 299.5, 305.98, 301.25, 299.73, 299.2, 298.62, 301.84, 299.6, 295.3, 299.3, 301.95, 296.3, 295.11, 295.12, 289.9, 283.51, 276.42, 264.22, 250.25, 236.66, 217.47, 199.75, 179.7, 160, 140.92, 113.53, 93.68, 69.71, 45.93, 20.87)
y <- c(-401.46, -375.44, -346.15, -320.2, -300.08, -274.12, -253.45, -226.4, -200.65, -171.62, -152.11, -125.19, -93.4, -74.79, -49.12, -28.73, 2.99, 25.65, 49.86, 72.87, 96.34, 120.4, 144.69, 168.06, 184.99, 205.11, 221.82, 238.3, 253.02, 267.19, 270.71, 285.86, 288.48, 292.9, 298.77)
zn <- cbind(x,y)Iteration Zero
# Initialization
x00 <- c(0, 0, 0, 0, 0, 0) # as we do not know the vehicle location
# Since is a guess, we set a very high estimate uncertainty
# The high estimate uncertainty results in a high Kalman Gain by giving a high weight to the measurement.
P00 <- diag(c(500, 500, 500, 500, 500, 500))Prediction
## [,1]
## vec1 0
## vec2 0
## vec3 0
## vec4 0
## vec5 0
## vec6 0
## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 1125.01 750.02 250.02 0.00 0.00 0.00
## vec2 750.02 1000.04 500.04 0.00 0.00 0.00
## vec3 250.02 500.04 500.04 0.00 0.00 0.00
## vec4 0.00 0.00 0.00 1125.01 750.02 250.02
## vec5 0.00 0.00 0.00 750.02 1000.04 500.04
## vec6 0.00 0.00 0.00 250.02 500.04 500.04
First Iteration
## [1] 301.50 -401.46
# Step 2 - Update
# The Kalman Gain calculation:
k1 <- P10 %*% t(H) %*% solve(H %*% P10 %*% t(H) + R); k1## [,1] [,2]
## vec1 0.9920636 0.0000000
## vec2 0.6613875 0.0000000
## vec3 0.2204742 0.0000000
## vec4 0.0000000 0.9920636
## vec5 0.0000000 0.6613875
## vec6 0.0000000 0.2204742
Therefore, we can see that the Kalman Gain for a position is 0.9921, which means that the weight of the first measurement is significantly higher than the weight of the estimation; that is the weight of the estimation is negligible.
## [,1]
## vec1 299.10716
## vec2 199.40832
## vec3 66.47299
## vec4 -398.27384
## vec5 -265.52061
## vec6 -88.51159
# Update the current estimate uncertainty
P11 <- (diag(6) - k1 %*% H) %*% P10 %*% t(diag(6) - k1 %*% H) + k1 %*% R %*% t(k1); P11## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 8.928572 5.952487 1.984268 0.000000 0.000000 0.000000
## vec2 5.952487 503.986173 334.679906 0.000000 0.000000 0.000000
## vec3 1.984268 334.679906 444.917029 0.000000 0.000000 0.000000
## vec4 0.000000 0.000000 0.000000 8.928572 5.952487 1.984268
## vec5 0.000000 0.000000 0.000000 5.952487 503.986173 334.679906
## vec6 0.000000 0.000000 0.000000 1.984268 334.679906 444.917029
## [,1]
## vec1 531.75198
## vec2 265.88131
## vec3 66.47299
## vec4 -708.05025
## vec5 -354.03220
## vec6 -88.51159
## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 972.7232 1236.4213 559.1427 0.0000 0.0000 0.0000
## vec2 1236.4213 1618.3030 779.6369 0.0000 0.0000 0.0000
## vec3 559.1427 779.6369 444.9570 0.0000 0.0000 0.0000
## vec4 0.0000 0.0000 0.0000 972.7232 1236.4213 559.1427
## vec5 0.0000 0.0000 0.0000 1236.4213 1618.3030 779.6369
## vec6 0.0000 0.0000 0.0000 559.1427 779.6369 444.9570
Generalization
multiKF <- function(transMat, processNoiseMat, H, R, iniX, iniP, zn){
# Initialization
x <- list(iniX)
p <- list(iniP)
k <- list(0)
# Prediction
x <- append(x, list(transMat %*% x[[1]]))
p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
for (i in 2:dim(zn)[1]){
# Measure
z <- c(zn[i-1, 1], zn[i-1, 2])
# print(paste0('z ', z))
# Update
k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
# print(paste0('k', k))
# Estimate the current state
x <- append(x, list(x[[length(x)]] + k[[i]] %*% (z - H %*% x[[length(x)]])))
# print(paste0('x', x))
# Update the current estimate uncertainty
p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
# print(paste0('p', p))
# Predict
x <- append(x, list(transMat %*% x[[length(x)]]))
# print(paste0('x', x))
p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
# print(paste0('p', p))
}
return(list(x, p, k))
}
result <- multiKF(transMat, processNoiseMat, H, R, x00, P00, zn)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]Visualization
Vehicle Position:
##
## Attaching package: 'dplyr'
## The following objects are masked from 'package:stats':
##
## filter, lag
## The following objects are masked from 'package:base':
##
## intersect, setdiff, setequal, union
##
## Attaching package: 'plotly'
## The following object is masked from 'package:ggplot2':
##
## last_plot
## The following object is masked from 'package:stats':
##
## filter
## The following object is masked from 'package:graphics':
##
## layout
# Take the estimates in the x axis
estimatesX <- c(300)
for (i in seq(3, 70, by = 2)){
estimatesX = append(estimatesX, list(x[[i]][1]))
}
# Take the estimates in the y axis
estimatesY <- c(-400)
for (i in seq(3, 70, by = 2)){
estimatesY = append(estimatesY, list(x[[i]][4]))
}# Dataframa with all the data
data <- data.frame(
estimX = unlist(estimatesX), # only the estimates
estimY = unlist(estimatesY),
measurements = zn
)
# True value?# Plot to compare the true value, measured values and estimates
plotPos <- ggplot(data) +
geom_path(aes(x = estimX, y = estimY, color = "Estimates")) +
geom_point(aes(x = estimX, y = estimY, color = "Estimates", shape = "Estimates")) +
geom_path(aes(x = measurements.x, y = measurements.y, color = "Measurements")) +
geom_point(aes(x = measurements.x, y = measurements.y, color = "Measurements", shape = "Measurements")) +
labs(title = "Vehicle Position",
x = "X(m)",
y = "Y(m)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green"), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18), name = "")
ggplotly(plotPos) %>% config(displayModeBar = TRUE)library(SIBER)
plot(x[[4]][1], x[[4]][4])
sigma2 <- rbind(c(p[[4]][4,4], 0),
c(0, p[[4]][4,4]))
mu <- c(x[[4]][1], x[[4]][4])
addEllipse(mu, sigma2, p.interval = 0.95, fill = "yellow", alpha = 0.2)## Warning in plot.xy(xy.coords(x, y), type = type, ...): "fill" is not a
## graphical parameter
## Warning in plot.xy(xy.coords(x, y), type = type, ...): "alpha" is not a
## graphical parameter
## [,1] [,2]
## [1,] 608.0936 -708.0502
## [2,] 607.9399 -703.2084
## [3,] 607.4794 -698.3860
## [4,] 606.7140 -693.6025
## [5,] 605.6467 -688.8772
## [6,] 604.2819 -684.2291
## [7,] 602.6250 -679.6769
## [8,] 600.6828 -675.2390
## [9,] 598.4630 -670.9332
## [10,] 595.9746 -666.7769
## [11,] 593.2276 -662.7867
## [12,] 590.2330 -658.9788
## [13,] 587.0030 -655.3685
## [14,] 583.5505 -651.9704
## [15,] 579.8894 -648.7980
## [16,] 576.0344 -645.8642
## [17,] 572.0012 -643.1809
## [18,] 567.8059 -640.7587
## [19,] 563.4654 -638.6075
## [20,] 558.9972 -636.7359
## [21,] 554.4193 -635.1515
## [22,] 549.7502 -633.8606
## [23,] 545.0086 -632.8685
## [24,] 540.2135 -632.1791
## [25,] 535.3845 -631.7951
## [26,] 530.5407 -631.7183
## [27,] 525.7019 -631.9488
## [28,] 520.8874 -632.4857
## [29,] 516.1167 -633.3269
## [30,] 511.4089 -634.4690
## [31,] 506.7831 -635.9074
## [32,] 502.2578 -637.6363
## [33,] 497.8512 -639.6487
## [34,] 493.5812 -641.9365
## [35,] 489.4648 -644.4905
## [36,] 485.5188 -647.3005
## [37,] 481.7589 -650.3551
## [38,] 478.2003 -653.6420
## [39,] 474.8573 -657.1480
## [40,] 471.7434 -660.8590
## [41,] 468.8712 -664.7600
## [42,] 466.2522 -668.8353
## [43,] 463.8969 -673.0685
## [44,] 461.8148 -677.4426
## [45,] 460.0144 -681.9399
## [46,] 458.5028 -686.5423
## [47,] 457.2861 -691.2314
## [48,] 456.3693 -695.9882
## [49,] 455.7561 -700.7935
## [50,] 455.4488 -705.6281
## [51,] 455.4488 -710.4724
## [52,] 455.7561 -715.3070
## [53,] 456.3693 -720.1123
## [54,] 457.2861 -724.8691
## [55,] 458.5028 -729.5582
## [56,] 460.0144 -734.1606
## [57,] 461.8148 -738.6579
## [58,] 463.8969 -743.0320
## [59,] 466.2522 -747.2652
## [60,] 468.8712 -751.3405
## [61,] 471.7434 -755.2415
## [62,] 474.8573 -758.9524
## [63,] 478.2003 -762.4584
## [64,] 481.7589 -765.7454
## [65,] 485.5188 -768.8000
## [66,] 489.4648 -771.6099
## [67,] 493.5812 -774.1640
## [68,] 497.8512 -776.4518
## [69,] 502.2578 -778.4642
## [70,] 506.7831 -780.1931
## [71,] 511.4089 -781.6315
## [72,] 516.1167 -782.7736
## [73,] 520.8874 -783.6148
## [74,] 525.7019 -784.1517
## [75,] 530.5407 -784.3822
## [76,] 535.3845 -784.3054
## [77,] 540.2135 -783.9214
## [78,] 545.0086 -783.2320
## [79,] 549.7502 -782.2399
## [80,] 554.4193 -780.9490
## [81,] 558.9972 -779.3646
## [82,] 563.4654 -777.4930
## [83,] 567.8059 -775.3418
## [84,] 572.0012 -772.9196
## [85,] 576.0344 -770.2363
## [86,] 579.8894 -767.3025
## [87,] 583.5505 -764.1301
## [88,] 587.0030 -760.7320
## [89,] 590.2330 -757.1217
## [90,] 593.2276 -753.3138
## [91,] 595.9746 -749.3236
## [92,] 598.4630 -745.1673
## [93,] 600.6828 -740.8615
## [94,] 602.6250 -736.4235
## [95,] 604.2819 -731.8714
## [96,] 605.6467 -727.2233
## [97,] 606.7140 -722.4980
## [98,] 607.4794 -717.7145
## [99,] 607.9399 -712.8921
## [100,] 608.0936 -708.0502
# Perform eigenvalue decomposition
eig <- eigen(sigma2)
# Calculate chi-square critical value for 95% confidence
chi_critical <- qchisq(0.95, df = 2)
# Calculate the ellipse axes lengths
ellipse_axes <- sqrt(chi_critical * eig$values)
# Calculate the rotation angle
rotation_angle <- atan2(eig$vectors[2, 1], eig$vectors[1, 1])
# Generate points for the ellipse
theta <- seq(0, 2 * pi, length.out = 100)
ellipse <- cbind(ellipse_axes[1] * cos(theta), ellipse_axes[2] * sin(theta))
rotation_matrix <- matrix(c(cos(rotation_angle), -sin(rotation_angle), sin(rotation_angle), cos(rotation_angle)), ncol = 2)
rotated_ellipse <- t(t(rotation_matrix %*% t(ellipse)))
# Plot the data with the confidence ellipse
ggplot() +
geom_point(aes(x[[4]][1], x[[4]][4])) +
stat_ellipse(type = "norm", level = 0.95, linetype = "dashed", color = "blue") +
theme_minimal()Vehicle X-axis velocity
# Take the velocity estimates in the x axis
estimatesX <- c()
for (i in seq(3, 70, by = 2)){
estimatesX = append(estimatesX, list(x[[i]][2]))
}
# Dataframa with all the data
dataVx <- data.frame(
recta = c(1:34),
estimX = unlist(estimatesX)
)
# True value?# Plot to compare the true value, measured values and estimates
plotVx <- ggplot(dataVx, aes(x = recta)) +
geom_path(aes(y = estimX, color = "Estimates")) +
geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
labs(title = "Vehicle X-axis velocity",
x = "Time(s)",
y = "Vx(m/s)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")
ggplotly(plotVx) %>% config(displayModeBar = TRUE)Vehicle Y-axis velocity
# Take the velocity estimates in the y axis
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
estimatesY = append(estimatesY, list(x[[i]][5]))
}
# Dataframa with all the data
dataVy <- data.frame(
recta = c(1:34),
estimY = unlist(estimatesY)
)
# True value?# Plot to compare the true value, measured values and estimates
plotVy <- ggplot(dataVy, aes(x = recta)) +
geom_path(aes(y = estimY, color = "Estimates")) +
geom_point(aes(y = estimY, color = "Estimates", shape = "Estimates")) +
labs(title = "Vehicle Y-axis velocity",
x = "Time(s)",
y = "Vy(m/s)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")
ggplotly(plotVy) %>% config(displayModeBar = TRUE)